

#ifndef __COMM_MSG_FUSED_STATIC_OBST_H__
#define __COMM_MSG_FUSED_STATIC_OBST_H__

#include "comm/comm_topic_name.h"
#include "comm/comm_base.h"
#include "comm/comm_car.h"
#include <comm_msg/coor2DArray.h>
#include <ros/ros.h>
#include <iostream>
#include <list>
#include <vector>

using namespace std;

/**************************************************************************************
功能描述: 融合静态障碍物消息接收器
修改记录:
**************************************************************************************/
class FUSED_STATIC_OBST_RECEIVER_C
{
public:
    FUSED_STATIC_OBST_RECEIVER_C(ros::NodeHandle &nh)
    {
        subscriber_ = nh.subscribe(TOPIC_NAME_FUSED_STATIC_OBST, 1, &FUSED_STATIC_OBST_RECEIVER_C::ReceiveMsgCallBack, this);

        objInvalidTimer_.SetOverTime(0.2);
    }

    INT32 Get(BOOL isMustIncludeForwardObsts, BOOL isMustIncludeBackwardObsts, vector<COORDINATE_2D_STRU> &vectObsts)
    {
        if (objInvalidTimer_.IsTimeOut()) {
            return -1;
        }

        BOOL isMsgNotMatch = (isMustIncludeForwardObsts && !isIncludeForwardObsts_) || (isMustIncludeBackwardObsts && !isIncludeBackwardObsts_);
        ST_LOG_ERR_SAMPLE_IF_TRUE(isMsgNotMatch, 20 * 60, "Not Match: NeedForward=%d, NeedBackward=%d, ExistForward=%d, ExistBackward=%d.", isMustIncludeForwardObsts, isMustIncludeBackwardObsts, isIncludeForwardObsts_, isIncludeBackwardObsts_);
        if (isMsgNotMatch) {
            return -1;
        }

        vectObsts = vectObsts_;
        return 0;
    }

private:
    ros::Subscriber             subscriber_;
    TIMER_C                     objInvalidTimer_;       /* 数据失效定时器，超时后数据无效 */
    BOOL                        isIncludeForwardObsts_;
    BOOL                        isIncludeBackwardObsts_;
    vector<COORDINATE_2D_STRU>  vectObsts_;

    void ReceiveMsgCallBack(const comm_msg::coor2DArray::ConstPtr &pstMsg)
    {
        R_ASSERT(pstMsg != NULL, NOTHING);

        objInvalidTimer_.Reset();

        isIncludeForwardObsts_  = pstMsg->isContainForwardObst;
        isIncludeBackwardObsts_ = pstMsg->isContainBackwardObst;

        vectObsts_.clear();
        vectObsts_.reserve(pstMsg->coor2DArray.size());

        for (UINT32 loop = 0; loop < pstMsg->coor2DArray.size(); ++loop) {
            COORDINATE_2D_STRU stObst;
            stObst.x = pstMsg->coor2DArray[loop].pos_x;
            stObst.y = pstMsg->coor2DArray[loop].pos_y;

            vectObsts_.push_back(stObst);
        }
    }
};


#endif

